/* INCLUDE */
#include <ros/ros.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <traj_utils/Flag.h>

struct traj_pt
{
    double x, y;
};

std::vector<traj_pt> traj_pts;
ros::Publisher traj_pts_puber;
geometry_msgs::PoseArray traj_pts_msg;
int now_pt_idx;
int traj_pts_num;
ros::Publisher cmd_puber;

void cmd_pub_callback(const ros::TimerEvent&)
{
    if (now_pt_idx < traj_pts_num-1)
    {
        now_pt_idx = now_pt_idx + 1;
    }
    geometry_msgs::Pose pt_temp;
    pt_temp.position.x = (traj_pts[now_pt_idx].x - traj_pts[0].x) / 2.0;
    pt_temp.position.y = (traj_pts[now_pt_idx].y - traj_pts[0].y) / 2.0;
    traj_pts_msg.poses.push_back(pt_temp);
    traj_pts_puber.publish(traj_pts_msg);
    std::cout << "pub one message!" << std::endl;
    std::cout << "vel:" << 5.0 * sqrt(pow(traj_pts[now_pt_idx].x - traj_pts[now_pt_idx-1].x, 2) +
                                pow(traj_pts[now_pt_idx].y - traj_pts[now_pt_idx-1].y, 2))
                        << std::endl;
    traj_utils::Flag cmd_msg;
    cmd_msg.cmd[0].position.x = (traj_pts[now_pt_idx].x - traj_pts[0].x) / 2.0;
    cmd_msg.cmd[0].position.y = (traj_pts[now_pt_idx].y - traj_pts[0].y) / 2.0;
    cmd_msg.cmd[0].velocity.x = (traj_pts[now_pt_idx].x - traj_pts[now_pt_idx-1].x) / 2.0 * 10.0;
    cmd_msg.cmd[0].velocity.y = (traj_pts[now_pt_idx].y - traj_pts[now_pt_idx-1].y) / 2.0 * 10.0;
    for (int i = 1; i < 6; i++)
    {
        if (now_pt_idx+i < traj_pts_num)
        {
            cmd_msg.cmd[i].position.x = (traj_pts[now_pt_idx+i].x - traj_pts[0].x) / 2.0;
            cmd_msg.cmd[i].position.y = (traj_pts[now_pt_idx+i].y - traj_pts[0].y) / 2.0;
            cmd_msg.cmd[i].velocity.x = (traj_pts[now_pt_idx+i].x - traj_pts[now_pt_idx+i-1].x) / 2.0 * 10.0;
            cmd_msg.cmd[i].velocity.y = (traj_pts[now_pt_idx+i].y - traj_pts[now_pt_idx+i-1].y) / 2.0 * 10.0;
        }
        else
        {
            cmd_msg.cmd[i].position.x = (traj_pts[traj_pts_num-1].x - traj_pts[0].x) / 2.0;
            cmd_msg.cmd[i].position.y = (traj_pts[traj_pts_num-1].y - traj_pts[0].y) / 2.0;
            cmd_msg.cmd[i].velocity.x = 0.0;
            cmd_msg.cmd[i].velocity.y = 0.0;
        }
    }
    cmd_puber.publish(cmd_msg);
}

int main(int argc, char **argv)
{
    /* 读入轨迹点数据 */
    std::cout << "loading traj information from txt" << std::endl;
    std::fstream f;
    f.open("/home/flag/flag-2024-jiangyin/src/traj_test_pkg/src/1.txt", std::ios::in);
    f >> traj_pts_num;
    traj_pts.resize(traj_pts_num);
    for (int i = 0; i < traj_pts_num; i++)
    {
        f >> traj_pts[i].x >> traj_pts[i].y;
    }

    /* 定时发布 */
    now_pt_idx = 0;
    ros::init(argc, argv, "traj_test_node");
    ros::NodeHandle nh("~");
    traj_pts_msg.header.frame_id = "world";
    traj_pts_puber = nh.advertise<geometry_msgs::PoseArray>("/traj_pt", 1000);
    cmd_puber = nh.advertise<traj_utils::Flag>("/ergodic_cmd", 1000);
    ros::Timer cmd_pub_timer;
    cmd_pub_timer = nh.createTimer(ros::Duration(0.1), cmd_pub_callback);
    ros::spin();

    return 0;
}